home *** CD-ROM | disk | FTP | other *** search
/ Amiga Format CD 46 / Amiga Format CD46 (1999-10-20)(Future Publishing)(GB)[!][issue 1999-12].iso / -in_the_mag- / reader_requests / scilab / demos / wheel2 / maple / wheel.f < prev    next >
Text File  |  1999-09-16  |  2KB  |  64 lines
  1. c      
  2. c     SUBROUTINE wheel
  3. c      
  4.       subroutine wheel(neq,t,z,zdot)
  5.         implicit double precision (t)
  6.         parameter (kn=3)
  7.         doubleprecision t,z(8),zdot(8),r,j(3),m
  8.         doubleprecision me3s(kn,kn)
  9.         doubleprecision const(kn,1),w(3*kn),rcond
  10.         integer i,k,neq,ierr
  11.         data g / 9.81/
  12.         data r / 1.0/
  13.         data m / 1.0/
  14.         data j / 0.3,0.4,1.0/
  15.       t1 = r**2
  16.       t2 = t1*m
  17.       t3 = t2+J(3)
  18.       t5 = cos(z(2))*t3
  19.       t9 = cos(2*z(2))
  20.          me3s(3,3) = t3
  21.          me3s(1,3) = t5
  22.          me3s(2,1) = 0
  23.          me3s(2,3) = 0
  24.          me3s(2,2) = J(1)+t2
  25.          me3s(3,1) = t5
  26.          me3s(1,2) = 0
  27.          me3s(3,2) = 0
  28.          me3s(1,1) = J(1)/2+t1*m*t9/2+t2/2+J(3)*t9/2+J(3)/2-J(1)*t9/2
  29.       t1 = r**2
  30.       t2 = z(4)**2
  31.       t4 = sin(2*z(2))
  32.       t5 = t2*t4
  33.       t11 = sin(z(2))
  34.       t12 = z(4)*z(6)
  35.       t34 = z(4)*t4
  36.          const(2,1) = -t1*m*t5/2+J(1)*t5/2-t1*t11*m*t12-r*cos(z(2))*m*g-
  37.      +J(3)*t11*t12-J(3)*t5/2
  38.          const(3,1) = t11*z(4)*z(5)*(2*t1*m+J(3))
  39.          const(1,1) = -z(5)*(-t1*m*t34-z(6)*t11*J(3)+J(1)*t34-J(3)*t34)
  40. c         
  41.         do 1000, i =1,kn ,1
  42.           zdot(i) = z(i+kn)
  43.  1000   continue
  44. c       
  45. c        we must solve  M z =const 
  46.         call dlslv(me3s,kn,kn,Const,kn,1,w, rcond,ierr,1)
  47.         if (ierr.ne.0) then
  48.           write(6,2000) 
  49.  2000     format('Ill conditioned matrix!')
  50.         endif
  51. c         
  52.         do 1001, i =1,kn ,1
  53.           zdot(kn+i) = const(i,1)
  54.  1001   continue
  55. c       
  56.       t2 = sin(z(1))
  57.         zdot(7) = r*cos(z(2))*t2*z(4)+r*sin(z(2))*cos(z(1))*z(5)+r*t2*z(
  58.      +6)
  59.       t2 = cos(z(1))
  60.         zdot(8) = -r*cos(z(2))*t2*z(4)+r*sin(z(2))*sin(z(1))*z(5)-r*t2*z
  61.      +(6)
  62.         return
  63.       end
  64.